#include "opencv2/opencv.hpp"
#include <QTime>
#include <QDebug>

#define START_ID      50
#define NO_TO_CAPTURE 50
#define USE_ACIRCLES  false

using namespace std;
using namespace cv;

/*
   Use this class to capture left/right images for stereo calibration
 */

int main( int, char* )
{
    for( int i = 0; i < NO_TO_CAPTURE; i ++ )
    {
        qDebug() << QString::number( i + 1 ) + "L.jpg";
        qDebug() << QString::number( i + 1 ) + "R.jpg";
    }
    VideoCapture cap0, cap1;
    if( cap0.open( 1 ) && cap1.open( 0 ) )
    {
        qDebug() << "Cameras #0 and #1 are opened";
    }
    else
    {
        qDebug() << "Error: Cameras #0 and #1 not found";
        return -1;
    }
    QTime time = QTime::currentTime();
    Mat img0, img1;
    int counter = START_ID;
    QString s0;
    QString s1;
    vector<Point2f> corners0;
    vector<Point2f> corners1;
    Size boardSize;
    if( USE_ACIRCLES )
        boardSize = Size( 4, 11 );
    else
        boardSize = Size( 9, 6 );
    namedWindow( "img0", 1 );
    namedWindow( "img1", 1 );

    while( counter < NO_TO_CAPTURE+START_ID+1 )
    {
        cap0 >> img0;
        cap1 >> img1;

        if( time.secsTo( QTime::currentTime() ) > 0 )
        {
            if( ( USE_ACIRCLES &&
                  findCirclesGrid( img0, boardSize, corners0, CALIB_CB_ASYMMETRIC_GRID )
                  &&
                  findCirclesGrid( img1, boardSize, corners1, CALIB_CB_ASYMMETRIC_GRID )
                 )
                 ||
                 ( findChessboardCorners( img0, boardSize, corners0 )
                 &&
                   findChessboardCorners( img1, boardSize, corners1 )
                 )

              )
            {
                s0 = QString::number( counter ) + "L.jpg";
                s1 = QString::number( counter ) + "R.jpg";
                imwrite( s0.toStdString(), img0 );
                imwrite( s1.toStdString(), img1 );
                time = QTime::currentTime();
                qDebug() << "Save Images" << s0 << s1 ;
                counter ++;
                drawChessboardCorners( img0, boardSize, Mat(corners0), true );
                drawChessboardCorners( img1, boardSize, Mat(corners1), true );
            }
        }
        imshow( "img0", img0 );
        imshow( "img1", img1 );
        if( waitKey( 30 ) >= 0 )
            break;
    }
    //fs.release();
    return 0;
}
